#include <error.h>
#include "modbusserver.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <mutex>
#include <time.h>
#include "../rtubuffer/rtubuffer.h"
#include <sys/select.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>

extern std::mutex response_mutex;
extern struct ResponseDataType responsedata;
extern std::mutex hpcmd_mutex;
extern struct RequestDataType highprioritycmd;
extern std::mutex buf_mutex;

/**
 * @brief ModbusTCPServer构造函数
 * 
 * @param cfg_in 配置变量副本
 */
modbusServer::modbusServer(BridgeConfig *cfg)
{
    tcp_handler = modbus_new_tcp("0.0.0.0", cfg->tcpconfig.port);
    if (tcp_handler == NULL)
    {
        log("Failed to creat tcp server!", 2);
        exit(-1);
    }
    modbussocket = modbus_tcp_listen(tcp_handler, 32);
    //设置socket池
    FD_ZERO(&refset);
    FD_SET(modbussocket, &refset);
    fdmax = modbussocket;
    modbus_set_debug(tcp_handler, 0);
    //modbus_set_indication_timeout(tcp_handler, 0, 50*1000);
    //回复消息由mapping自动生成，两种寄存器存储区数据指针均指向data
    mapping = modbus_mapping_new(0, 0, 0, 0);
    mapping->tab_input_registers = data;
    mapping->tab_registers = data;
    //mapping nb设置为最大回复长度，同时也是单次发送所需的最多数据量
    mapping->nb_input_registers = cfg->bridgeconfig.maxnb;
    mapping->nb_registers = cfg->bridgeconfig.maxnb;
    debuglog = cfg->debuglog;
    loglevel = cfg->loglevel;
}

modbusServer::~modbusServer()
{
    modbus_free(tcp_handler);
}

/**
 * @brief ModbusTCPServer主处理循环（TCPServer线程）
 * 
 */
void modbusServer::exec()
{
    int current_socket = 0;
    bool wait_reply = false;
    int rc;
    uint8_t query[MODBUS_TCP_MAX_ADU_LENGTH];
    uint8_t slave;
    uint8_t func;
    uint16_t addr;
    uint16_t nb;
    for (;;)
    {
        rdset = refset;
        if (select(fdmax + 1, &rdset, NULL, NULL, NULL) == -1)
        {
            log("Server select failure", 2);
            return;
        }
        for (current_socket = 0; current_socket <= fdmax; current_socket++)
        {
            //轮询处理socket
            if (!FD_ISSET(current_socket, &rdset))
                continue;
            if (current_socket == modbussocket)
            {
                //接受新传入
                log("new tcp connection request", 0);
                socklen_t addrlen;
                struct sockaddr_in clientaddr;
                int newfd;
                addrlen = sizeof(clientaddr);
                memset(&clientaddr, 0, sizeof(clientaddr));
                newfd = accept(modbussocket, (struct sockaddr *)&clientaddr, &addrlen);
                if (newfd == -1)
                    log("New connection accept error.", 2);
                else
                {
                    FD_SET(newfd, &refset);
                    if (newfd > fdmax)
                        fdmax = newfd;
                    log("New connection accept.", 1);
                }
            }
            else
            {
                //使用libmodbus处理已经存在的连接
                modbus_set_socket(tcp_handler, current_socket);
                rc = modbus_receive(tcp_handler, query);
                if (rc > 0)
                {
                    log("new tcp message income", 0);
                    slave = query[6];
                    func = query[7];
                    addr = query[8] << 8 | query[9];
                    nb = query[10] << 8 | query[11];
                    if (func == 0x03 || func == 0x04)
                    {
                        //寄存器读取
                        buf_mutex.lock();
                        bool exist = ModbusBufferRead(slave, func, addr, nb, data);
                        buf_mutex.unlock();
                        if (exist)
                        { //如果缓存值可用则直接回复
                            log("reply directly", 1);
                            mapping->start_input_registers = addr;
                            mapping->start_registers = addr;
                            modbus_reply(tcp_handler, query, rc, mapping);
                        }
                        else
                        { //不可用则转发RTU查询
                            log("wait reply", 1);
                            wait_reply = true;
                            hpcmd_mutex.lock();
                            highprioritycmd.enable = true;
                            highprioritycmd.cmd.func = func;
                            highprioritycmd.cmd.addr = addr;
                            highprioritycmd.cmd.slave = slave;
                            highprioritycmd.cmd.nb = nb;
                            hpcmd_mutex.unlock();
                        }
                    }
                    else if (func == 0x06) //写单个寄存器
                    {                      //写入命令回复并转发
                        log("reply and write", 1);
                        hpcmd_mutex.lock();
                        highprioritycmd.enable = true;
                        highprioritycmd.cmd.func = func;
                        highprioritycmd.cmd.addr = addr;
                        highprioritycmd.cmd.slave = slave;
                        highprioritycmd.cmd.nb = nb;
                        hpcmd_mutex.unlock();
                        //回复
                        data[0] = nb;
                        mapping->start_input_registers = addr;
                        mapping->start_registers = addr;
                        modbus_reply(tcp_handler, query, rc, mapping);
                    }
                    else if (func == 0x10) //写多个寄存器
                    {                      //写入命令回复并转发
                        log("reply and write multi", 1);
                        hpcmd_mutex.lock();
                        highprioritycmd.enable = true;
                        highprioritycmd.cmd.func = func;
                        highprioritycmd.cmd.addr = addr;
                        highprioritycmd.cmd.slave = slave;
                        highprioritycmd.cmd.nb = nb;
                        for (int i = 0; i < nb; i++)
                            data[i] = highprioritycmd.req_data[i] = query[13 + 2 * i] << 8 | query[14 + 2 * i];
                        hpcmd_mutex.unlock();
                        //回复
                        mapping->start_input_registers = addr;
                        mapping->start_registers = addr;
                        modbus_reply(tcp_handler, query, rc, mapping);
                    }
                    while (wait_reply)
                    {
                        //查询RTU回复结果
                        struct ResponseDataType res;
                        response_mutex.lock();
                        res = responsedata;
                        responsedata.enable = false;
                        response_mutex.unlock();
                        if (res.enable)
                        {
                            wait_reply = false;
                            if (!res.miss)
                            {
                                //回复有效
                                log("reply received : send", 1);
                                ModbusBufferRead(slave, func, addr, nb, data);
                                mapping->start_input_registers = addr;
                                mapping->start_registers = addr;
                                modbus_reply(tcp_handler, query, rc, mapping);
                            }
                            else
                                log("reply received : rtu timeout", 2);
                        }
                        timespec t;
                        t.tv_sec = 0;
                        t.tv_nsec = 1000000;
                        nanosleep(&t, NULL);
                    }
                }
                else if (rc == -1)
                {
                    //远端关闭连接
                    log("tcp connection closed", 1);
                    close(current_socket);
                    FD_CLR(current_socket, &refset);
                    if (current_socket == fdmax)
                        fdmax--;
                }
            }
            timespec t2;
            t2.tv_sec = 0;
            t2.tv_nsec = 10000;
            nanosleep(&t2, NULL);
        }
    }
}